
#include <Wire.h>
#include <LSM303.h>
#include <L3G4200D.h>

double Fpitchangle;
unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;
float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)
int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
  float c_magnetom_x;
  float c_magnetom_y;
  float c_magnetom_z;
#define MOTOR1 3          // left motor PWM
#define MOTOR2 11         // right motor PWM
#define MOTOR3 9         // left motor direction
#define MOTOR4 10         // right motor direction
#define MOTOR_FRONT 0
#define MOTOR_REAR 1
#define MOTOR_LEFT 2
#define MOTOR_RIGHT 3
#define RX_ROLL 4
#define RX_PITCH 3
#define RX_YAW 2
#define RX_THROTTLE 1  
#define RX_GEAR 5
#define RX_AUX 6
#define SENSOR_PITCH 0
#define SENSOR_ROLL 1
#define SENSOR_YAW 2
#define SENSOR_ZAXIS 2
#define MINCOMMAND 1000
#define MIDCOMMAND 1100
#define MAXCOMMAND 1200
#define PWM_FREQUENCY 300 // in Hz  
#define PWM_PRESCALER 8  
#define PWM_COUNTER_PERIOD (F_CPU/PWM_PRESCALER/PWM_FREQUENCY) 
#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction
#define PWM2RAD 0.002
#define ATTITUDE_SCALING (-0.75 * PWM2RAD)
#define ZAXIS_PID_IDX               2
#define ATTITUDE_XAXIS_PID_IDX      3
#define ATTITUDE_YAXIS_PID_IDX      4
#define ATTITUDE_GYRO_XAXIS_PID_IDX 6
#define ATTITUDE_GYRO_YAXIS_PID_IDX 7

#define OUTPUT_MOTOR 0
#define OUPUT_ACCELD 0
#define OUTPUT_MINMAX 0
#define OUTPUT_GRAPH 1
#define OUTPUT_STUFF 0


int windupGuard = 1000;
int Rout=0;
int Lout=0;
int Fout=0;
int Bout=0;
int RMOTOR=0;
int LMOTOR=0;
int FMOTOR=0;
int BMOTOR=0;
int RMOTORPID=0;
int LMOTORPID=0;
int FMOTORPID=0;
int BMOTORPID=0;
int YAWPID=0;
int throttle = 1000;
float headingHold = 0;

int motorAxisCommandRoll = 0;
int motorAxisCommandPitch = 0;
int motorAxisCommandYaw = 0;


int RXCH[6];
int RXCHval[6];
int RXCHmin[6];
int RXCHmax[6];
volatile int RXSG[6];
int RXOK[6];
int PWMSG[6];

LSM303 compass;
L3G4200D gyro;

struct Motor {
  int command[4];
  int axisCommand[3];
  boolean armed;
  int minimum;
} motor = {
  { MINCOMMAND, MINCOMMAND, MINCOMMAND, MINCOMMAND },
  { 0, 0, 0 },
  false,
  1000
};

struct GyroData {
  int zero[3];
  int min[3];
  int max[3];
  int prev[3];
  int value[3];
  int factor;
} 
gyrod = { 
  {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , 3 };

struct AccelData {
  int zero[3];
  int min[3];
  int max[3];
  int prev[3];
  int value[3];
  int factor;
} 
acceld = { 
  {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , 3 };

struct MagData {
  int zero[3];
  int min[3];
  int max[3];
  int prev[3];
  int value[3];
  int factor;
} 
magd = { 
  {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , {    
    0, 0, 0      }  
  , 3 };

struct Level {
  int rollRatio;
  int pitchRatio;
  int rollCount;
  int pitchCount;
  int rollAdjust;
  int pitchAdjust;
  boolean enabled;
} 
level = { 
  0, 0, 0, 0, 0, 0, false };
struct Timing {
  unsigned long current;
  unsigned long previous;
  unsigned long serial;
  unsigned long control;
  unsigned long sensor;
  unsigned long rx;
  int delta;
} 
time = { 
  0, 0, 0, 0, 0, 0, 0};
struct PID {
  int P, I, D;
  int lastPosition;
  int integratedError;
} pid[5];


void pwmSetup(){//just run once at setup
  // Set all motor control pins to outputs.
  pinMode(MOTOR1, OUTPUT);
  pinMode(MOTOR2, OUTPUT);
  pinMode(MOTOR3, OUTPUT);
  pinMode(MOTOR4, OUTPUT);

  DDRB = DDRB | B00001110;                                  // Set ports to output PB1-3   
  DDRD = DDRD | B00001000;                                  // Set port to output PD3  

  TCCR1A = (1<<WGM11)|(1<<COM1A1)|(1<<COM1B1);  
  TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);  
  ICR1 = PWM_COUNTER_PERIOD;  
  // Init PWM Timer 2   8bit                               // WGMn1 WGMn2 = Mode ? Fast PWM, TOP = 0xFF ,Update of OCRnx at BOTTOM    
  TCCR2A = (1<<WGM20)|(1<<WGM21)|(1<<COM2A1)|(1<<COM2B1);  // Clear OCnA/OCnB on compare match, set OCnA/OCnB at BOTTOM (non-inverting mode)    
  TCCR2B = (1<<CS22)|(1<<CS21);                            // Prescaler set to 256, that gives us a resolution of 16us     
  // TOP is fixed at 255             
  // Output_PWM_Frequency = 244hz = 16000000/(256*(1+255)) = Clock_Speed / (Prescaler * (1 + TOP))  

  OCR2B = 900 / 16 ; // 1000-2000 to 128-256   
  OCR1A = 900 * 2 ;  
  OCR1B = 900 * 2 ;  
  OCR2A = 900 / 16 ;  

  Serial.println(OCR1A);
  Serial.println(OCR1B);
  Serial.println(OCR2A);
  Serial.println(OCR2B);
  while(millis() < 5000){

  }

}

void setup() {
PIDsetup();
pinMode(12,OUTPUT);
digitalWrite(12,HIGH);
  Serial.begin(115200);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  compass.writeMagReg(LSM303_MR_REG_M, 0x00);
  gyro.enableDefault();
  pwmSetup();
  setupPID(2,1,0);
  RCINIT();
}

void loop() {
  time.current = millis();
  time.delta = (int) (time.current - time.previous);
  time.previous = time.current;

  if(time.current > (time.rx + 100)) { // 10Hz
    RC_READ();
    if (RXCHval[RX_THROTTLE] < 700){ RXCHval[RX_THROTTLE] = 1000; }
    if (RXCHval[RX_ROLL] < 700){ RXCHval[RX_ROLL] = 1500; }
    if (RXCHval[RX_PITCH] < 700){ RXCHval[RX_PITCH] = 1500; }
    if (RXCHval[RX_YAW] < 700){ RXCHval[RX_YAW] = 1500; }
    time.rx = time.current;
  }
  if(time.current > (time.sensor + 2)) {
    dtMP = (Now - lastreadMP)/1000.0; //compute the time delta since last MP read, in sec.
    lastreadMP = Now;

    acceld.prev[0] = acceld.value[0];
    acceld.prev[1] = acceld.value[1];
    acceld.prev[2] = acceld.value[2];
    gyrod.prev[0] = gyrod.value[0];
    gyrod.prev[1] = gyrod.value[1];
    gyrod.prev[2] = gyrod.value[2];

    magd.prev[0] = compass.m.x;
    magd.prev[1] = compass.m.y;
    magd.prev[2] = compass.m.z;

    compass.read();
    gyro.read();
    compass.readMag();
  
//    acceld.value[0] = smooth((int)compass.a.y, acceld.prev[0], acceld.factor);
//    acceld.value[1] = smooth((int)compass.a.x, acceld.prev[1], acceld.factor);
//    acceld.value[2] = smooth((int)compass.a.z, acceld.prev[2], acceld.factor);    
//
//    gyrod.value[0] = smooth((int)gyro.g.y, gyrod.prev[0], gyrod.factor);
//    gyrod.value[1] = smooth((int)gyro.g.x, gyrod.prev[1], gyrod.factor);
//    gyrod.value[2] = smooth((int)gyro.g.z, gyrod.prev[2], gyrod.factor);

    magd.value[0] = smooth((int)compass.m.x,magd.prev[0],3);
    magd.value[1] = smooth((int)compass.m.y,magd.prev[1],3);
    magd.value[2] = smooth((int)compass.m.z,magd.prev[2],3);
    
    gyrod.value[0] = smooth((int)gyro.g.y, gyrod.prev[0], 3);
    gyrod.value[1] = smooth((int)gyro.g.x, gyrod.prev[1], 3);
    gyrod.value[2] = smooth((int)gyro.g.z, gyrod.prev[2], 3);
    
    acceld.value[0] = smooth((int)compass.a.x, acceld.prev[0], 3);
    acceld.value[1] = smooth((int)compass.a.y, acceld.prev[1], 3);
    acceld.value[2] = smooth((int)compass.a.z, acceld.prev[2], 3);

 
//    Serial.println(compFilter(acceld.prev[1], gyrod.value[1], acceld.value[1], dtMP));

    for(int i=0;i<3;i++){
      if( gyrod.min[i] > gyrod.value[i]){
        gyrod.min[i] = gyrod.value[i];
      }
      if( gyrod.max[i] < gyrod.value[i]){
        gyrod.max[i] = gyrod.value[i];
      }      
      if( acceld.min[i] > acceld.value[i]){
        acceld.min[i] = acceld.value[i];
      }
      if( acceld.max[i] < acceld.value[i]){
        acceld.max[i] = acceld.value[i];
      }
      if( magd.min[i] > magd.value[i]){
        magd.min[i] = magd.value[i];
      }
      if( magd.max[i] < magd.value[i]){
        magd.max[i] = magd.value[i];
      }            
    }
  }
  int pwm=0;  

  //Control Loop
  if(time.current > time.control + 2) { // 500Hz

   int accXangle = map(acceld.value[SENSOR_ROLL],-1027,1027,-180,180);
   int accYangle = map(acceld.value[SENSOR_PITCH],-1027,1027,-180,180);
   int accZangle = map(acceld.value[SENSOR_YAW],-1027,1027,-180,180);
   int gyroZangle = map(gyrod.value[SENSOR_YAW],-32768,32768,-180,180);   

  const float commandedYaw = constrain(RXCHval[RX_YAW] + radians(headingHold), -PI, PI);
  motorAxisCommandYaw = updatePID(commandedYaw, magd.value[0], &pid[ZAXIS_PID_IDX]);
  
// Calculate Pitch and Roll angles
double CFroll = compFilter(acceld.prev[1], gyrod.value[1], acceld.value[1], dtMP);
double CFpitch = compFilter(acceld.prev[0], gyrod.value[0], acceld.value[0], dtMP);
double CFyaw = compFilter(magd.prev[0], gyrod.value[SENSOR_YAW], magd.value[0], dtMP);

  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  
  cos_roll = cos(CFroll);
  sin_roll = sin(CFroll);
  cos_pitch = cos(CFpitch);
  sin_pitch = sin(CFpitch);

// adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
  c_magnetom_x = (float)(magd.value[0] - SENSOR_SIGN[6]*magd.min[0]) / (magd.max[0] - magd.min[0]) - SENSOR_SIGN[6]*0.5;
  c_magnetom_y = (float)(magd.value[1] - SENSOR_SIGN[7]*magd.min[1]) / (magd.max[1] - magd.min[1]) - SENSOR_SIGN[7]*0.5;
  c_magnetom_z = (float)(magd.value[2] - SENSOR_SIGN[8]*magd.min[2]) / (magd.max[2] - magd.min[2]) - SENSOR_SIGN[8]*0.5;

// Tilt compensated Magnetic filed X:
  MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
  // Tilt compensated Magnetic filed Y:
  MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
  // Magnetic Heading
  float MAG_Heading = atan2(-MAG_Y,MAG_X);  


    level.rollRatio = (int) ( ((long) CFroll * 1000) / CFyaw);
    level.pitchRatio = (int) ( ((long) CFpitch * 1000) / CFyaw);

//Serial.println(level.rollRatio);
//Serial.println(level.pitchRatio);


 double pitchangle = (double)(atan2(CFpitch,sqrt(sq(CFyaw)+sq(CFroll)))*180/3.14);
 double rollangle = (double)(atan2(CFroll,sqrt(sq(CFyaw)+sq(CFpitch)))*180/3.14);
 double Gpitchangle = (double)(atan2(gyrod.value[SENSOR_PITCH],sqrt(sq(gyrod.value[SENSOR_YAW])+sq(gyrod.value[SENSOR_ROLL])))*180/3.14);
 double Grollangle = (double)(atan2(gyrod.value[SENSOR_ROLL],sqrt(sq(gyrod.value[SENSOR_YAW])+sq(gyrod.value[SENSOR_PITCH])))*180/3.14);


 
 
 
//Serial.println (rollangle);
//Serial.println (map(RXCHval[RX_ROLL],1000,2000,180,-180));
//Serial.println (pitchangle);
//Serial.println (map(RXCHval[RX_PITCH],1000,2000,-180,180));

//    float rollAttitudeCmd  = updatePID((RXCHval[RX_ROLL] - 1500) * ATTITUDE_SCALING, accXangle, &pid[ATTITUDE_XAXIS_PID_IDX]);
    float rollAttitudeCmd  = updatePID(map(RXCHval[RX_ROLL],1000,2000,180,-180), rollangle, &pid[ATTITUDE_XAXIS_PID_IDX]);
    float pitchAttitudeCmd = updatePID(map(RXCHval[RX_PITCH],1000,2000,-180,180), pitchangle, &pid[ATTITUDE_YAXIS_PID_IDX]);
    
// Autolevel control      
    motorAxisCommandRoll   = updatePID(rollAttitudeCmd, gyrod.value[SENSOR_ROLL], &pid[ATTITUDE_GYRO_XAXIS_PID_IDX]);
    motorAxisCommandPitch  = updatePID(pitchAttitudeCmd, gyrod.value[SENSOR_PITCH], &pid[ATTITUDE_GYRO_YAXIS_PID_IDX]);

// Working commands for manual control
//    motorAxisCommandRoll   = updatePID(map(RXCHval[RX_ROLL],1000,2000,180,-180), rollangle, &pid[ATTITUDE_GYRO_XAXIS_PID_IDX]);
//    motorAxisCommandPitch  = updatePID(map(RXCHval[RX_PITCH],1000,2000,-180,180), pitchangle, &pid[ATTITUDE_GYRO_YAXIS_PID_IDX]);    


    throttle = map(RXCHval[RX_THROTTLE],1000,2000,1000,1200);
    
        const int throttleCorrection = abs(motorAxisCommandYaw*2/4);
        motor.command[MOTOR_FRONT] = constrain((throttle - throttleCorrection) - motorAxisCommandPitch + (YAW_DIRECTION * motorAxisCommandYaw),1000,1200);
        motor.command[MOTOR_REAR] =  constrain((throttle - throttleCorrection) + motorAxisCommandPitch + (YAW_DIRECTION * motorAxisCommandYaw),1000,1200);
        motor.command[MOTOR_RIGHT] = constrain((throttle - throttleCorrection) + motorAxisCommandRoll  + (YAW_DIRECTION * motorAxisCommandYaw),1000,1200);
        motor.command[MOTOR_LEFT] =  constrain((throttle - throttleCorrection) - motorAxisCommandRoll  + (YAW_DIRECTION * motorAxisCommandYaw),1000,1200);

    
    OCR2B =  motor.command[MOTOR_FRONT] /16;
    OCR2A =  motor.command[MOTOR_REAR] /16;
    OCR1B =  motor.command[MOTOR_LEFT] *2;
    OCR1A =  motor.command[MOTOR_RIGHT] *2;

#if OUTPUT_GRAPH ==1
   Serial.print(CFroll);
   Serial.print(", ");
   Serial.print(CFpitch);
   Serial.print(", ");
//   Serial.println(CFyaw);
//   Serial.println(MAG_Heading);
   Serial.println(motorAxisCommandYaw);
#endif

  }
  // SERIAL PORT LOOP
  if(time.current > (time.serial + 100)) { // 10Hz
    PrintData();
    time.serial = time.current;
  }
}



